import rospy
import socket
from geometry_msgs.msg import Point
from std_msgs.msg import Int64,Bool
from acupoint_msgs.msg import AcupointCoordinate
import json


class JakaCommunicate:
    def __init__(self):
       
        self.client=socket.socket(socket.AF_INET,socket.SOCK_STREAM)
        try:
            self.client.connect(('0.0.0.0',56789))
            self.r = rospy.Rate(10)
            while not rospy.is_shutdown():
                if self.flag>=0:
                    x=self.acupoint_msg.acupoints[self.flag].x
                    y=self.acupoint_msg.acupoints[self.flag].y
                    z=self.acupoint_msg.acupoints[self.flag].z
                    data={'flag':self.flag,'x':x,'y':y,'z':z}
                    data_json=json.dumps(data)
                    self.client.send(data_json.encode())
                    recv_data=self.client.recv(1024)
                    if not recv_data:
                        break
                    jaka_arrived_msg=Bool()
                    jaka_arrived_msg=True
                    jaka_arrived_pub.publish(jaka_arrived_msg)
                else:
                    data={'flag':-1,'x':0,'y':0,'z':0}
                    data_json=json.dumps(data)
                    self.client.send(data_json.encode())
                    recv_data=self.client.recv(1024)
                    if not recv_data:
                        break
                    jaka_arrived_msg=Bool()
                    jaka_arrived_msg=True
                    jaka_arrived_pub.publish(jaka_arrived_msg)
                move_flag=-1
            self.r.sleep()
        except:
            print("connect failed")
        finally:
            self.client.close()
        
        



    def acupointCallback(self,msg):
        self.acupoint_msg=deepcopy(msg)
    
    def boolCallback(self,msg):
        self.flag=msg.data
    


if __name__ == "__main__":
    rospy.init_node('nuc_communicate')
    client=socket.socket(socket.AF_INET,socket.SOCK_STREAM)
    client.connect(('10.161.229.76',56789))
    data={'flag':0,'x':1,'y':1,'z':1}
    data_json=json.dumps(data)
    client.send(data_json.encode())
    recv_data=client.recv(1024)
   
    print(recv_data)
    client.close()
   



    

    
